package sma.iss.calc;

import java.util.ArrayList;
import java.util.List;

import org.slf4j.Logger;
import org.slf4j.LoggerFactory;

import sma.iss.Position;
import sma.iss.dto.Try;
import sma.iss.util.AngleUtil;
import sma.iss.util.IssException;
import sma.nasa.ConstraintsChecker;

/**
 * Calculate a path
 * 
 * @author smarsoll
 * 
 */
public class PathOptimizer {

	/** Logger for this class. */
	private final static Logger LOGGER = LoggerFactory.getLogger(PathOptimizer.class);

	/** tolerance */
	static final double EPS = ConstraintsChecker.EPS;

	double maxAcc, maxSpeed;

	public PathOptimizer(double maxSpeed, double maxAcc) {
		super();
		this.maxAcc = maxAcc;
		this.maxSpeed = maxSpeed;
	}

	private static final PathOptimizer OPT_SARG = new PathOptimizer(ConstraintsChecker.MAX_SARJ_SPEED - EPS,
			ConstraintsChecker.MAX_SARJ_ACC);
	private static final PathOptimizer OPT_BGA = new PathOptimizer(ConstraintsChecker.MAX_BGA_SPEED - EPS, ConstraintsChecker.MAX_BGA_ACC);

	private static final int NB_ROTERS = 10;

	public static final PathOptimizer getPathOptimizer(int numRot) {
		if (numRot < 2) {
			// 0.0050
			return OPT_SARG;
		} else {
			// 0.01
			return OPT_BGA;
		}
	}

	/**
	 * Validate a transition
	 * 
	 * @param fromPos
	 * @param toPos
	 * @return
	 */
	public final static boolean transitionOkForPosition(Position fromPos, Position toPos) {
		int deltaminute = fromPos.getDelatMinuteToPos(toPos);

		for (int numRot = 0; numRot < NB_ROTERS; numRot++) {
			PathOptimizer opt = PathOptimizer.getPathOptimizer(numRot);
			double currentAngle = fromPos.getAngle(numRot);
			if (currentAngle < 0.0 || currentAngle >= 360.0) {
				throw new RuntimeException("Invalid angle " + currentAngle + " for rot " + numRot + " in " + toPos);
			}
			if (!opt.canMakeTransBool(currentAngle, fromPos.getSpeed(numRot), toPos.getAngle(numRot), toPos.getSpeed(numRot),
					deltaminute * 60.0)) {
				// LOGGER.info("{}, {}, {}, {}, {}, ", currentAngle, currentPos.getSpeed(numRot),
				// position.getAngle(numRot),
				// position.getSpeed(numRot), deltaSecCheck);
				return false;
			}
		}
		return true;
	}

	/**
	 * Simple movement :<br>
	 * accelerate at maximum, then stay finalSpeed until the end
	 * 
	 * @param currentSpeed
	 * @param d
	 * @param deltaSeconde2
	 * @return
	 */
	public final double calculateSimpleShift(double initialSpeed, double finalSpeed, double deltaSecond) {
		double a1;
		if (finalSpeed >= initialSpeed) {
			a1 = maxAcc;
		} else {
			a1 = -maxAcc;
		}
		double tA = (finalSpeed - initialSpeed) / a1;
		return tA * initialSpeed + tA * (finalSpeed - initialSpeed) / 2.0 + (deltaSecond - tA) * finalSpeed;
	}


	/**
	 * 
	 * @param angle1
	 * @param speed1
	 * @param angle2
	 * @param speed2
	 * @param nbSec
	 * @param secInter
	 * @return angle and speed
	 */
	public double[] interpolate(double angle1, double speed1, double angle2, double speed2, double nbSec, double secInter) {
		double shift = angle2 - angle1;
		if (shift < -180.0)
			shift += 360.0;
		if (shift > 180.0)
			shift -= 360.0;
		// shift =

		if (this.canMakeTrans(angle1, speed1, angle2, speed2, nbSec)[0] != 1.0) {
			LOGGER.error("impossible transition {} {} {} {} {}", angle1, speed1, angle2, speed2, nbSec);
			throw new IssException("the path to interpolate is not possible");
		}

		if (speed2 < speed1) {
			LOGGER.debug("interpolate revert");
			double[] retNeg = interpolate(360.0 - angle1, -speed1, 360.0 - angle2, -speed2, nbSec, secInter);
			retNeg[0] = 360.0 - retNeg[0];
			retNeg[1] = -retNeg[1];
			return retNeg;
		}

		// iterate to find constant speed Vc
		double precision = 0.00000001;

		List<Try> list = new ArrayList<Try>();
		
		// constant speed, starting value
		double vC = maxSpeed;
		list.add(new Try(vC, calculateShiftForVc(speed1, speed2, nbSec, vC)));
		 vC = -maxSpeed;
		list.add(new Try(vC, calculateShiftForVc(speed1, speed2, nbSec, vC)));
			
		double shiftToFind = angle2 - angle1;
		if (shiftToFind < -180.0)
			shiftToFind += 360.0;
		if (shiftToFind > 180.0)
			shiftToFind -= 360.0;
		LOGGER.debug("shiftToFind={} from speed {} to {}", shiftToFind, speed1, speed2);
		// i< maximum number of loops
		for (int i = 0; i < 300; i++) {
			// linearise new Vc
			vC = list.get(0).param + (list.get(1).param - list.get(0).param) / (list.get(1).result - list.get(0).result)
					* (shiftToFind - list.get(0).result);
			if (Math.abs(vC) > maxSpeed + EPS) {
				LOGGER.error("out of bounds vC=" + vC);
				throw new RuntimeException("vC=" + vC);
			}
			double shiftCalculated = calculateShiftForVc(speed1, speed2, nbSec, vC);
			LOGGER.debug("for {} shiftCalculated is {}", vC, shiftCalculated);
			double diff = Math.abs(shiftCalculated - shiftToFind);
			if (diff < precision) {
				LOGGER.debug("value found diff={} at {}", diff, i);
				break;
			}
			// remove bad result:
			if (Math.abs(list.get(0).result - shiftToFind) >= Math.abs(list.get(1).result - shiftToFind)) {
				list.remove(0);
			} else {
				list.remove(1);
			}
			// add new result
			list.add(new Try(vC, shiftCalculated));
		}

		double[] ret = calculateInterpolateForVc(speed1, speed2, nbSec, vC, secInter);
		// change shift to angle:
		ret[0] = AngleUtil.addAngle(angle1, ret[0]);

		return ret;
	}

	/**
	 * 
	 * @param angle1
	 * @param speed1
	 * @param angle2
	 * @param speed2
	 * @param nbSec
	 * @param vC
	 * @param secInter
	 * @return shift and speed
	 */
	private double[] calculateInterpolateForVc(double speed1, double speed2, double nbSec, double vC, 
			double secInter) { 
		LOGGER.debug("speed1 {}, speed2  {}, nbSec  {}, vC  {}, secInter  {}", speed1, speed2, nbSec, vC, secInter);
		if (speed1 > speed2) {
			throw new RuntimeException();
		}
		// t1 = 0
		// t2 = nbSec

		double a1 = maxAcc;
		double a2 = maxAcc;
		if (vC > speed2) {
			a2 = -maxAcc;
		} else if (vC < speed1) {
			a1 = -maxAcc;
		}

		double tA = (vC - speed1) / a1;
		double tB = (tA * a1 + speed1 - speed2) / a2 + nbSec;
		double[] ret = new double[2];
		if (secInter < tA) {
			ret[0] = speed1 * secInter + a1 * secInter * secInter / 2.0;
			ret[1] = speed1 + a1 * secInter;
		} else if (secInter < tB) {
			ret[0] = speed1 * tA + a1 * tA * tA / 2.0 + vC * (secInter - tA);
			ret[1] = vC;
		} else {
			ret[0] = speed1 * tA + a1 * tA * tA / 2.0 + vC * (secInter - tA) + a2 * (secInter - tB) * (secInter - tB) / 2.0;
			ret[1] = vC + a2 * (secInter - tB);
		}
		// ret[0] = AngleUtil.addAngle(ret[0], angle1);
		// check result
		if (Math.abs(ret[1]) > maxSpeed + EPS) {
			LOGGER.error("ret[1]=" + ret[1]);
			assert false;
		}
		return ret;

	}

	private double calculateShiftForVc(double speed1, double speed2, double nbSec, double vC) {
		if (speed1 > speed2) {
			throw new RuntimeException();
		}
		// t1 = 0
		// t2 = nbSec

		// double a1 = maxAcc;
		// double a2 = maxAcc;
		// if (vC > speed2) {
		// a2 = -maxAcc;
		// } else if (vC < speed1) {
		// a1 = -maxAcc;
		// }
		//
		// double tA = (vC - speed1) / a1;
		// double tB = (tA * a1 + speed1 - speed2) / a2 + nbSec;
		//
		// double shift = (tA * speed1 + tA * vC + nbSec * vC + nbSec * speed2 + tB * vC - tB * speed2) / 2.0;
		// LOGGER.debug("vc={}, shift calc={}", vC, shift);
		double[] ret = calculateInterpolateForVc(speed1, speed2, nbSec, vC, nbSec);
		LOGGER.debug("vc={}, shift calc 2={}", vC, ret[0]);
		return ret[0];
	}

	/**
	 * If it is possible, element 0 of return will be 1.0, otherwise 0.0.<br>
	 * Element 1 of return will contain the minimum amount of rotation necessary to make such transition.<br>
	 * (This is only defined if transition is possible
	 * 
	 * @param angle1
	 * @param speed1
	 * @param angle2
	 * @param speed2
	 * @param nbSec
	 * @return
	 */
	public double[] canMakeTrans(double angle1, double speed1, double angle2, double speed2, double nbSec) {
		return processTransition(angle1, speed1, angle2, speed2, this.maxSpeed, this.maxAcc, nbSec);
	}

	public boolean canMakeTransBool(double angle1, double speed1, double angle2, double speed2, double nbSec) {
		return processTransition(angle1, speed1, angle2, speed2, this.maxSpeed, this.maxAcc, nbSec)[0] == 1.0;
	}

	/**
	 * Verifies whether a transition from (angle1, speed1) to (angle2, speed2) is possible within TIME_PER_STATE seconds
	 * assuming |speed| <= maxSpeed and |acceleration| <= maxAcc.<br>
	 * If it is possible, element 0 of return will be 1.0, otherwise 0.0.<br>
	 * Element 1 of return will contain the minimum amount of rotation necessary to make such transition. (This is only
	 * defined if transition is possible.)
	 */
	// =============================================
	double[] processTransition(double angle1, double speed1, double angle2, double speed2, double maxSpeed, double maxAcc, double nbSec)
	// =============================================
	{
		// make sure speed1 >= 0, to reduce the number of cases
		if (speed1 < 0)
			return processTransition(-angle1, -speed1, -angle2, -speed2, maxSpeed, maxAcc, nbSec);

		// determine angular shift we need to make
		double shift = angle2 - angle1;
		if (shift < -180.0)
			shift += 360.0;
		if (shift > 180.0)
			shift -= 360.0;

		// check validity
		double[] res = new double[] { 0.0, 0.0 };

		if (!canMakeTransition(shift, speed1, speed2, -maxSpeed, maxSpeed, maxAcc, nbSec))
			return res;

		res[0] = 1.0;
		res[1] = Math.abs(shift);

		// check if some extra rotation is needed
		double pos, neg;
		if (speed2 >= 0) {
			// can we always keep speed positive?
			if (canMakeTransition(shift, speed1, speed2, 0, maxSpeed, maxAcc, nbSec))
				return res;

			// minimize rotation at positive speeds
			double t1 = speed1 / maxAcc;
			double t2 = speed2 / maxAcc;
			pos = path(t1, 0, maxAcc) + path(t2, 0, maxAcc);
			neg = pos - shift;
		} else {
			double t1 = speed1 / maxAcc;
			double t2 = (-speed2) / maxAcc;
			pos = path(t1, 0, maxAcc);
			neg = path(t2, 0, maxAcc);
			double midShift = pos - neg;
			if (shift < midShift) {
				// minimize rotation at positive speeds
				neg = pos - shift;
			} else {
				// minimize rotation at negative speeds
				pos = neg + shift;
			}
		}

		res[1] = pos + neg;

		return res;
	}

	/**
	 * can we shift shift degrees ?
	 * 
	 * @param shift
	 * @param speed1
	 * @param speed2
	 * @param nbSec
	 * @return true if possible
	 */
	public boolean canMakeTrans(double shift, double speed1, double speed2, double nbSec) {
		return canMakeTransition(shift, speed1, speed2, -this.maxSpeed, this.maxSpeed, this.maxAcc, nbSec);
	}

	/**
	 * Verifies whether it's possible to make an angular shift of "shift" within TIME_PER_STATE seconds<br>
	 * if initial speed is "speed1", final speed is "speed2", minSpeed <= speed <= maxSpeed and |acceleration| <=
	 * maxAcc.
	 * 
	 * @param TIME_PER_STATE
	 */
	// =============================================
	boolean canMakeTransition(double shift, double speed1, double speed2, double minSpeed, double maxSpeed, double maxAcc,
			double TIME_PER_STATE)
	// =============================================
	{
		// simple acceleration check
		if (Math.abs((speed2 - speed1) / TIME_PER_STATE) > maxAcc + EPS)
			return false;

		double minShift = 0.0, maxShift = 0.0;

		// find minimum possible angular shift
		double t1 = (speed1 - minSpeed) / maxAcc;
		double t2 = TIME_PER_STATE - (speed2 - minSpeed) / maxAcc;
		if (t1 <= t2) {
			minShift += path(t1, speed1, -maxAcc);
			minShift += path(t2 - t1, minSpeed, 0);
			minShift += path(TIME_PER_STATE - t2, minSpeed, maxAcc);
		} else {
			double t = (speed1 - speed2 + TIME_PER_STATE * maxAcc) / 2.0 / maxAcc;
			minShift += path(t, speed1, -maxAcc);
			minShift += path(TIME_PER_STATE - t, speed1 - maxAcc * t, maxAcc);
		}

		// find maximum possible angular shift
		t1 = (maxSpeed - speed1) / maxAcc;
		t2 = TIME_PER_STATE - (maxSpeed - speed2) / maxAcc;
		if (t1 <= t2) {
			maxShift += path(t1, speed1, maxAcc);
			maxShift += path(t2 - t1, maxSpeed, 0);
			maxShift += path(TIME_PER_STATE - t2, maxSpeed, -maxAcc);
		} else {
			double t = (speed2 - speed1 + TIME_PER_STATE * maxAcc) / 2.0 / maxAcc;
			maxShift += path(t, speed1, maxAcc);
			maxShift += path(TIME_PER_STATE - t, speed1 + maxAcc * t, -maxAcc);
		}

		// validate
		return (minShift <= shift && shift <= maxShift + EPS);
	}

	// Returns path, given time, initial speed and acceleration.
	// =============================================
	double path(double t, double v0, double a)
	// =============================================
	{
		return v0 * t + a * t * t / 2.0;
	}

}
